#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include<iostream>
#include <opencv2/opencv.hpp>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <opencv2/highgui.hpp>
#include<opencv2/core/core.hpp>


using namespace std;
using namespace cv;
Mat cameraMatrix=(Mat_<double>(3,3)<<1128.048344, 0., 339.421769,0.,1127.052190, 236.535242,0., 0., 1.);
Mat distCoeffs=(Mat_<double>(3,3)<<-0.568429, 0.514592, -0.000126, 0.000500, 0.000000);
vector<Vec4i> hierarchy;
int main(int argv, char **argc)
{

    // Ptr<BackgroundSubtractor> pBackSub;

    // pBackSub = createBackgroundSubtractorMOG2();
    VideoCapture capture("./water.avi");
    if (!capture.isOpened())
    {
        //error in opening the video input
        printf("could not load image..\n");
        return false;
    }
    Mat frame, hsv, fgMask, mask,cornerpoint;
    while (true)
    {
        capture >> frame;
        // frame = imutils.resize(frame, 600)

        if (frame.empty())
            break;

        cvtColor(frame, hsv, COLOR_BGR2HSV);
        inRange(hsv, Scalar(0, 43, 46), Scalar(26, 255, 255), mask);
        threshold(hsv,hsv,70,230,CV_THRESH_BINARY);
        // GaussianBlur(frame, frame, Size(11, 11), 3, 3);
        //闭操作联通+膨胀
        dilate(mask, mask, Mat(), Point(-1, -1), 14);
        erode(mask, mask, Mat(), Point(-1, -1), 19);
        dilate(mask, mask, Mat(), Point(-1, -1), 3);
        // pBackSub->apply(mask, fgMask);
        // fgMask=mask.clone();
       
        vector<vector<Point> >cont;
        findContours(mask, cont, CV_RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
        // drawContours(conimg,cont,-1,Scalar(255),1);
        // imshow("",conimg  );
        imshow("mask",mask  );
        mask.copyTo(cornerpoint);
        vector<RotatedRect> rect(cont.size());
        Mat savepoint;
        for (int i = 0;i < cont.size(); i++)
        {
            rect[i] = minAreaRect(cont[i]);
            Point2f vertices[4];      //定义矩形的4个顶点
            if(rect[i].center.y>80)
            {
            rect[i].points(vertices);
            } //计算矩形的4个顶点
            for (int j = 0; j < 4; j++)
                line(cornerpoint, vertices[i], vertices[(i + 1) % 4], Scalar(0, 255, 0), 1);
            
        }
// imshow("",cornerpoint);
        // Mat kernel = getStructuringElement(MORPH_RECT, Size(15, 15), Point(-1, -1));
        // vector<Point2f> corners; //提供初始角点的坐标位置和精确的坐标的位置
        // int maxcorners = 200;
        // double qualityLevel = 0.01; //角点检测可接受的最小特征值
        // double minDistance = 10;    //角点之间最小距离
        // int blockSize = 3;          //计算导数自相关矩阵时指定的领域范围
        // double k = 0.04;            //权重系数
        // goodFeaturesToTrack(fgMask, corners, maxcorners, qualityLevel, minDistance, Mat(), blockSize, false, k);

        // // 提取视频row and column
        // int wid = frame.rows;
        // int hei = frame.cols;
        // //提取corner points
        // int uplx = 0;
        // int uply = 0;
        // int downlx = 0;
        // int downly = 0;
        // int uprx = 0;
        // int upry = 0;
        // int downrx = 0;
        // int downry = 0;
        //遍历点集
        // if (corners.size() != 0)
        // {

        //冒泡算法
        //     int xx = 0;
        //     int yy = 0;
        //     int len = corners.size();
        //     for (int i = 0; i < len - 1; ++i)
        //         for (int j = 0; j < len - 1 - i; ++j)
        //             if (corners[j].x > corners[j + 1].x)
        //             {
        //                 xx = corners[j].x;
        //                 yy = corners[j].y;

        //                 corners[j].x = corners[j + 1].x;
        //                 corners[j].y = corners[j + 1].y;
        //                 corners[j + 1].x = xx;
        //                 corners[j + 1].y = yy;

        //             }

        //     int bool1=1;
        //     int minw = corners[0].x;
        //     int maxw = corners[len - 1].x;
        //     uplx = corners[0].x;
        //     uply = corners[0].y;
        //     uprx = corners[0].x;
        //     upry = corners[0].y;
        //     downlx = corners[len-1].x;
        //     downly = corners[len-1].y;
        //     downrx = corners[len-1].x;
        //     downry = corners[len-1].y;
        //     for (int i = 0; i < corners.size(); i++)
        //     {
        //         if (bool1==1&&corners[i].x==minw&&uply>corners[i].y)
        //         {
        //             uplx = corners[i].x;
        //             uply=corners[i].y;
        //             bool1=0;
        //         }
        //     }

        // // }
        // cout<<corners<<endl;
        // cout << "左上坐标：" << uplx << "," << uply << endl;
        // cout << "左上坐标：" << uplx << "," << uply << endl;
        // cout << "右上坐标：" << uprx<< "," << upry << endl;
        // cout << "右下坐标：" << downrx << "," << downry << endl;
        // cout << "左下坐标：" << downlx << "," << downly << endl;

        // //绘制角点
        // RNG rng(12345); //控制随机生成数,彩色点标记
        // for (unsigned i = 0; i < corners.size(); i++)
        // {

        //     circle(frame, corners[i], 2, Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)), -1, 8, 0);
        //     // cout << "角点坐标：" <<i<< corners[i] << endl;
        // }

        // imshow("Frame", frame);
        // imshow("FG Mask", fgMask);

        //get the input from the keyboard
        int keyboard = waitKey(10);
        if (keyboard == 'q' || keyboard == 27)
            break;
    }
    capture.release();
    return (0);
}